// Copyright 2006-2012 Brendan Burns
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#ifndef __NMC_LOW_H__
#define __NMC_LOW_H__

//Servo Module Command set:
#define	RESET_POS	  0x00	//Reset encoder counter to 0 (0 bytes)
#define	SET_ADDR	  0x01	//Set address and group address (2 bytes)
#define	DEF_STAT	  0x02	//Define status items to return (1 byte)
#define	READ_STAT	  0x03	//Read value of current status items
#define	LOAD_TRAJ  	  0x04	//Load trahectory date (1 - 14 bytes)
#define START_MOVE	  0x05	//Start pre-loaded trajectory (0 bytes)
#define SET_GAIN	  0x06  //Set servo gain and control parameters (13 or 14)
#define	STOP_MOTOR 	  0x07	//Stop motor (1 byte)
#define	IO_CTRL		  0x08	//Define bit directions and set output (1 byte)
#define SET_HOMING	  0x09  //Define homing mode (1 byte)
#define	SET_BAUD	  0x0A 	//Set the baud rate (1 byte)
#define CLEAR_BITS	  0x0B  //Save current pos. in home pos. register (0 bytes)
#define SAVE_AS_HOME      0x0C	//Store the input bytes and timer val (0 bytes)
#define ADD_PATHPOINT     0x0D  //Adds path points for path mode
#define	NOP		  0x0E	//No operation - returns prev. defined status (0 bytes)
#define HARD_RESET	  0x0F	//RESET - no status is returned

#define	FAST_PATH	  0x40  //0 = 30 or 60 Hz path execution, 1 = 60 or 120 Hz

//Servo Module STATUSITEMS bit definitions:
#define	SEND_POS	  0x01	//4 bytes data
#define	SEND_AD		  0x02	//1 byte
#define	SEND_VEL	  0x04	//2 bytes
#define SEND_AUX	  0x08	//1 byte
#define SEND_HOME	  0x10	//4 bytes
#define SEND_ID		  0x20	//2 bytes
#define SEND_PERROR	  0x40  //2 bytes
#define SEND_NPOINTS      0x80  //1 byte

#define CHKSUM_ERROR      0x2

//Servo Module STOP_MOTOR control byte bit definitions:
#define	AMP_ENABLE	  0x01	//1 = raise amp enable output, 0 = lower amp enable
#define MOTOR_OFF	  0x02	//set to turn motor off
#define STOP_ABRUPT       0x04	//set to stop motor immediately
#define STOP_SMOOTH	  0x08  //set to decellerate motor smoothly
#define STOP_HERE	  0x10	//set to stop at position (4 add'l data bytes required)
#define ADV_FEATURE   0x20  //enable features in ver. CMC

//Servo Module ADD_PATHPOINT frequency definitions
#define P_30HZ		  30	//30 hz path resolution
#define P_60HZ		  60    //60 hs path resolution
#define P_120HZ		  120   //120 hs path resolution

//Servo Module LOAD_TRAJ control byte bit definitions:
#define	LOAD_POS	  0x01	//+4 bytes
#define LOAD_VEL	  0x02	//+4 bytes
#define	LOAD_ACC	  0x04	//+4 bytes
#define LOAD_PWM	  0x08	//+1 byte
#define ENABLE_SERVO      0x10  //1 = servo mode, 0 = PWM mode
#define VEL_MODE	  0x20	//1 = velocity mode, 0 = trap. position mode
#define REVERSE		  0x40  //1 = command neg. PWM or vel, 0 = positive
#define MOVE_REL	  0x40  //1 = move relative, 0 = move absolute
#define START_NOW	  0x80  //1 = start now, 0 = wait for START_MOVE command

typedef struct {
  long position;
  short int velocity;
  short int error;
  char npoints;
  char ad;
  
  char ioctrl;
  long last_point;
} picservo_t;

typedef struct {
  long foo;
} picstep_t;

typedef struct {
  int status;
  int type;
  int version;
  char group;
  char status_byte;
  union 
  {
    picservo_t servo;
    picstep_t stepper;
  } mod;
} module_t;

typedef struct nmc_port_t {
  int fd;
  module_t *modules;
} nmc_port;


/**
 * Print the flags in a status byte
 * \param byte The status byte.
 **/
void print_status(char byte);

/**
 * Reset the home position (Pg. 23)
 * \param fd The file to read/write
 * \param addr The address of the module
 **/
int nmc_reset(nmc_port *port, int addr);

/**
 * Set the group address of a module (Pg. 24)
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param id The new group id
 * \param leader Is this module the leader of the group?
 **/
int nmc_set_group_internal(nmc_port *port, int addr, int id, int leader);

/**
 * Define the status data sent back (Pg. 25)
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param flags The status flags
 **/
int nmc_define_status(nmc_port *port, int addr, int flags);

/**
 * Read the status of a module (Pg. 26)
 * \param fd The file to read/write
 * \param addr The addres of the module
 **/
int nmc_read_status(nmc_port *port, int ad);

/**
 * Read the status of a module (Pg. 26)
 * \param fd The file to read/write
 * \param addr The addres of the module
 * \param flags The status items to get.
 **/
int nmc_send_read_status(nmc_port *port, int addr, char flags);

/**
 * Send a motion to the controller. (Pg. 27)
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param mode The mode of the command
 * \param pos The commanded position (if mode is set position)
 * \param vel The commanded velocity (if mode is set velocity)
 * \param acc The commanded acceleration (if mode is set acc.)
 * \param pwm The commanded pwm (if mode is raw)
 **/
int nmc_servo_load_traj(nmc_port *port, char addr, char mode, long pos, long vel, long acc, char pwm, bool wait);

/**
 * Start a motion previously loaded via Load Trajectory (Pg. 30)
 * \param fd The file to read/write
 * \param group The group of modules to start
 * \param leader The leader of the group.
 **/
int nmc_start_motion(nmc_port *port, char group, char leader);


/**
 * Set the gains on the PID controller (Pg. 31)
 * \param fd The file to read/write
 * \param kp Propotional gain
 * \param ki Integral gain
 * \param kd Derivative gain
 * \param integration_limit Maximum value for integral term
 * \param output_limit Maximum pwm output
 * \param current_limit Maximum current output 
 * \param error_limit Maximum error value
 * \param rate_divisor See section 4.3 of manual
 * \param deadband_compensation See section 4.3 of manual
 **/
int nmc_set_gain(nmc_port *port, uint ix, short int Kp, short int Ki, short int Kd, 
		 short int integration_limit, char output_limit, 
		 char current_limit, short int error_limit, char rate_divisor, 
		 char deadband_compensation, char step_reate_multiplier);


/**
 * Adjust the motor parameters (Pg. 33) 
 * Does not support fourth mode.
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param mode The motor mode
 **/
int nmc_motor_stop(nmc_port *port, int addr, char mode);

/**
 * Change the baud rate of communication (Pg. 37)
 * \param fd The file to read/write
 * \param baud The new speed
 **/
int nmc_change_baud(nmc_port *port, int baud);

/**
 * Save the current position as home (Pg. 39)
 * \param fd The file to read/write
 * \param group The group to send the command to
 * \param leader The address of the group leader
 **/
int nmc_save_home(nmc_port *port, char group, char leader);


/**
 * Start a stored path (Pg. 40)
 * \param fd The file to read/write
 * \param group The group holding the path
 * \param leader The leader of the group
 **/
int nmc_start_path_internal (nmc_port *port, char group, char leader);

/**
 * Add path points (Pg. 40) Maximum 7 points.
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param count The number of points
 * \param path The set of points.
 **/
int nmc_add_pathpoints(nmc_port *port, int addr, int count, int *path, char freq);

/**
 * No Op, simply returns status. (Pg. 42)
 * \param fd The file to read/write
 * \param addr The address
 **/
int nmc_no_op_internal(nmc_port *port, char addr);

/**
 * Hard reset (Pg. 43) Resets baud to 19200
 * \param fd The file to read/write
 * \param group The group
 **/
void nmc_hard_reset(nmc_port *port, char group);

/**
 * Connect to the nmc network.
 * \param port The file to open
 * \param baud The desired baudrate
 * \param nummod Output parameter, number of modules
 * \returns The file descriptor
 **/
nmc_port *nmc_connect(const char *port, int baud, int *nummod);

/**
 * Initialize a module, must be called before a module
 * is referenced.
 * \param fd The file to read/write
 * \param addr The address of the module
 **/
int nmc_init_module(nmc_port *port, int addr);

/**
 * Reset a modules encoder to zero
 * \param fd The file to read/write
 * \param addr The address of the module
 **/
int nmc_zero_encoder(nmc_port *port, int addr);

#endif
